import threading
import time
import serial
import redis
import json


rover = serial.Serial(port='/dev/ttyACM0')

print(rover.baudrate)


r = redis.Redis(db=0)
r.set('process_3', 'on')
server_r = redis.Redis(host='117.185.46.193', password='lwj380279011', db=0, socket_timeout=3)

rtcm_p = server_r.pubsub()
rtcm_p.subscribe('rtcm')


def feed_rtk(rtcm_p, rover):
    while r.get('process_3').decode("utf-8") == 'on':
        try:
            server_r = redis.Redis(host='117.185.46.193', password='lwj380279011', db=0, socket_timeout=1)
            rtcm_p = server_r.pubsub()
            rtcm_p.subscribe('rtcm')

            for m in rtcm_p.listen():
                # print(m['data'])
                # print(m['data'])
                rover.write(m['data'])
                # print('wrote')
                if r.get('process_3').decode("utf-8") == 'off':
                    break
        except Exception as e:
            print(e)



def hex_reverse(s):
    return "".join(reversed([s[i:i + 2] for i in range(0, len(s), 2)]))


if __name__ == '__main__':
    x = threading.Thread(target=feed_rtk, args=(rtcm_p, rover))
    x.start()

    r.delete('gps_queue')

    while r.get('process_3').decode("utf-8") == 'on':
        ubx_message = rover.read_all().hex()

        if len(ubx_message) == 72:
            print(ubx_message)

            ubx_message = ubx_message[12:]

            lon = ubx_message[8:8 + 8]
            lat = ubx_message[16:16 + 8]
            hMSL = ubx_message[24:24 + 8]
            hAcc = ubx_message[40:40 + 8]
            vAcc = ubx_message[48:48 + 8]

            hAcc = int(hex_reverse(hAcc), 16)
            lon = int(hex_reverse(lon), 16) / 10000000
            lat = int(hex_reverse(lat), 16) / 10000000
            hMSL = int(hex_reverse(hMSL), 16)
            vAcc = int(hex_reverse(vAcc), 16)

            r.lpush('gps_queue', json.dumps([lat, lon, hMSL, hAcc, vAcc, time.time()]))
            r.ltrim('gps_queue', 0, 99)

            # server_r.set('lat', lat)
            # server_r.set('lon', lon)
            # server_r.set('hAcc', hAcc)
            # server_r.set('gps_timestamp', time.time())

            print(lon, lat, hAcc)